/*********************************************************************
*
* Software License Agreement (BSD License)
*
*  Copyright (c) 2008, Willow Garage, Inc.
*  All rights reserved.
*
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions
*  are met:
*
*   * Redistributions of source code must retain the above copyright
*     notice, this list of conditions and the following disclaimer.
*   * Redistributions in binary form must reproduce the above
*     copyright notice, this list of conditions and the following
*     disclaimer in the documentation and/or other materials provided
*     with the distribution.
*   * Neither the name of the Willow Garage nor the names of its
*     contributors may be used to endorse or promote products derived
*     from this software without specific prior written permission.
*
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
*  POSSIBILITY OF SUCH DAMAGE.
*
* Author: Eitan Marder-Eppstein
*         Mike Phillips (put the planner in its own thread)
*********************************************************************/
//                                         整体文件结构介绍
/*********************************************************************
 * nav_core:提供接口，能够实现插件更换算法的主要包,核心功能包
*  move_base:机器人移动导航框架(导航最主要的逻辑框架)
-----
*  costmap_2d:代价地图实现
*  voxel_grid:三维代价地图
*  map_server:提供代价地图的管理服务
-----
*  amcl:根据机器人自身的里程数值以及地图特征，利用粒子滤波修正机器人在已知的地图内的位置-定位
*  robot_pose_ekf:综合里程计、GPS、imu数据，通过拓展卡尔曼滤波进行位置估计
*  fake_localization:主要用来做定位仿真
-----
*  carrot_planner:很简单的全局路径规划器，生成的路径为目标点到机器人当前点的连线上的点
*  nav_fn:基于栅格的，实现了Dijkstra和A*  不建议再使用了
*  global_planner:重新实现了Dijkstra和A*全局规划算法,可以看作navfn的改进版
-----
*  base_local_planner:实现了Trajectory Rollout和DWA两种局部规划算法
*  dwa_local_planner:实现了DWA局部规划算法，可以看作是base_local_planner的改进版本
-----
*   clear_costmap_recovery:无法规划路径的恢复算法,实现了清除代价地图的恢复行为
*   move_slow_and_clear:缓慢移动的恢复行为
*   rotate_recovery:旋转恢复策略实现包
*********************************************************************/
#include <move_base/move_base.h>
#include <move_base_msgs/RecoveryStatus.h>
#include <cmath>

#include <boost/algorithm/string.hpp>
#include <boost/thread.hpp>

#include <geometry_msgs/Twist.h>

#include <tf2_geometry_msgs/tf2_geometry_msgs.h>

// 整体流程：
// move_base 功能包中 global_planner 订阅 move_base_simple/goal 话题，拿到目标点位置，进行全局规划，新建线程，
// 调用 makePlan() 函数进行全局规划，获得全局路径，之后传给 controller_plan，之后传给局部规划器tc_，若局部规划器选择的是dwa，
// 则调用局部轨迹规划器dwa中的setPlan() 函数 进行局部规划。利用局部轨迹规划器 computeVelocityCommands() 函数计算速度，下发给底盘

namespace move_base {
  // 初始化列表中几个重要的参数：
  // planner_costmap_ros_ 是用于全局导航的地图， controller_costmap_ros_ 是局部导航用的地图，
  // bgp_loader_ 是 global planner ， blp_loader_ 是local planner 二者的声明为：
  // pluginlib::ClassLoader<nav_core::BaseGlobalPlanner> bgp_loader_;
  // pluginlib::ClassLoader<nav_core::BaseLocalPlanner> blp_loader_; 是属于一个模板类ClassLoader，实例化为BaseGlobalPlanner或者BaseLocalPlanner。
  // 这个构造函数直接把 move_base 框中的所有元素都加载好了，包括全局规划器，局部规划器，全局代价地图，局部代价地图，还有恢复行为
  MoveBase::MoveBase(tf2_ros::Buffer& tf) :
    tf_(tf),
    as_(NULL),
    planner_costmap_ros_(NULL), controller_costmap_ros_(NULL),
    bgp_loader_("nav_core", "nav_core::BaseGlobalPlanner"),
    blp_loader_("nav_core", "nav_core::BaseLocalPlanner"),
    recovery_loader_("nav_core", "nav_core::RecoveryBehavior"),
    planner_plan_(NULL), latest_plan_(NULL), controller_plan_(NULL),
    runPlanner_(false), setup_(false), p_freq_change_(false), c_freq_change_(false), new_global_plan_(false) {
    // as_维护movebase的actionServer状态机，并且新建了一个executeCb线程(move_base的大循环)
    //一旦有目标点传进来，就会开始执行让机器人动起来，其中的过程包括：目标点的转换，路径规划，发布速度给让机器人
    as_ = new MoveBaseActionServer(ros::NodeHandle(), "move_base", boost::bind(&MoveBase::executeCb, this, _1), false);

    ros::NodeHandle private_nh("~");
    ros::NodeHandle nh;

    recovery_trigger_ = PLANNING_R;

    // 设置move_base_node中的全局参数，如果在yaml文件中没有设置对应的参数，则默认使用param函数中最后的参数
    std::string global_planner, local_planner;
    private_nh.param("base_global_planner", global_planner, std::string("navfn/NavfnROS"));//这里可以修改使用A*和dijkstra算法，使用global_planner包  global_planner/GlobalPlanner
    private_nh.param("base_local_planner", local_planner, std::string("base_local_planner/TrajectoryPlannerROS"));//dwa_local_planner/DWAPlannerROS
    private_nh.param("global_costmap/robot_base_frame", robot_base_frame_, std::string("base_link"));
    private_nh.param("global_costmap/global_frame", global_frame_, std::string("map"));
    private_nh.param("planner_frequency", planner_frequency_, 0.0);
    private_nh.param("controller_frequency", controller_frequency_, 20.0);
    private_nh.param("planner_patience", planner_patience_, 5.0);
    private_nh.param("controller_patience", controller_patience_, 15.0);
    private_nh.param("max_planning_retries", max_planning_retries_, -1);  // disabled by default

    private_nh.param("oscillation_timeout", oscillation_timeout_, 0.0);
    private_nh.param("oscillation_distance", oscillation_distance_, 0.5);

    // make_plan 服务的参数
    private_nh.param("make_plan_clear_costmap", make_plan_clear_costmap_, true);
    private_nh.param("make_plan_add_unreachable_goal", make_plan_add_unreachable_goal_, true);

    // 这三个plan都是global_plan，最终由controller_plan_ 传给 local planner： if(!tc_->setPlan(*controller_plan_))
    planner_plan_ = new std::vector<geometry_msgs::PoseStamped>();
    latest_plan_ = new std::vector<geometry_msgs::PoseStamped>();
    controller_plan_ = new std::vector<geometry_msgs::PoseStamped>();
    
    planner_thread_ = new boost::thread(boost::bind(&MoveBase::planThread, this));// 新建planner线程
    vel_pub_ = nh.advertise<geometry_msgs::Twist>("cmd_vel", 1);// 下发速度命令给基座
    current_goal_pub_ = private_nh.advertise<geometry_msgs::PoseStamped>("current_goal", 0 );// 将当前目标点发送出去

    ros::NodeHandle action_nh("move_base");
    action_goal_pub_ = action_nh.advertise<move_base_msgs::MoveBaseActionGoal>("goal", 1);
    recovery_status_pub_= action_nh.advertise<move_base_msgs::RecoveryStatus>("recovery_status", 1);

    //we'll provide a mechanism for some people to send goals as PoseStamped messages over a topic
    //they won't get any useful information back about its status, but this is useful for tools like nav_view and rviz
    ros::NodeHandle simple_nh("move_base_simple");
    goal_sub_ = simple_nh.subscribe<geometry_msgs::PoseStamped>("goal", 1, boost::bind(&MoveBase::goalCB, this, _1));// 这里订阅了/move_base_simple/goal这个话题，获取从rviz过来的目标点

    //we'll assume the radius of the robot to be consistent with what's specified for the costmaps
    private_nh.param("local_costmap/inscribed_radius", inscribed_radius_, 0.325);
    private_nh.param("local_costmap/circumscribed_radius", circumscribed_radius_, 0.46);
    private_nh.param("clearing_radius", clearing_radius_, circumscribed_radius_);
    private_nh.param("conservative_reset_dist", conservative_reset_dist_, 3.0);

    private_nh.param("shutdown_costmaps", shutdown_costmaps_, false);
    private_nh.param("clearing_rotation_allowed", clearing_rotation_allowed_, true);
    private_nh.param("recovery_behavior_enabled", recovery_behavior_enabled_, true);

    //create the ros wrapper for the planner's costmap... and initializer a pointer we'll use with the underlying map
    planner_costmap_ros_ = new costmap_2d::Costmap2DROS("global_costmap", tf_);
    planner_costmap_ros_->pause();

    // 初始化全局规划器，并创建实例
    try {
      planner_ = bgp_loader_.createInstance(global_planner);
      planner_->initialize(bgp_loader_.getName(global_planner), planner_costmap_ros_);
    } catch (const pluginlib::PluginlibException& ex) {
      ROS_FATAL("Failed to create the %s planner, are you sure it is properly registered and that the containing library is built? Exception: %s", global_planner.c_str(), ex.what());
      exit(1);
    }

    //create the ros wrapper for the controller's costmap... and initializer a pointer we'll use with the underlying map
    controller_costmap_ros_ = new costmap_2d::Costmap2DROS("local_costmap", tf_);
    controller_costmap_ros_->pause();

    // 创建局部规划器，并创建实例
    try {
      tc_ = blp_loader_.createInstance(local_planner);
      ROS_INFO("Created local_planner %s", local_planner.c_str());
      tc_->initialize(blp_loader_.getName(local_planner), &tf_, controller_costmap_ros_); // MoveBase中加载局部路径规划器
    } catch (const pluginlib::PluginlibException& ex) {
      ROS_FATAL("Failed to create the %s planner, are you sure it is properly registered and that the containing library is built? Exception: %s", local_planner.c_str(), ex.what());
      exit(1);
    }

    // Start actively updating costmaps based on sensor data  开启costmap基于传感器数据的更新
    planner_costmap_ros_->start();
    controller_costmap_ros_->start();

    //advertise a service for getting a plan  接着发布了两个服务make_plan，clear_costmaps
    make_plan_srv_ = private_nh.advertiseService("make_plan", &MoveBase::planService, this);
    // 声明一个用于清除代价地图的服务
    clear_costmaps_srv_ = private_nh.advertiseService("clear_costmaps", &MoveBase::clearCostmapsService, this);

    //if we shutdown our costmaps when we're deactivated... we'll do that now
    if(shutdown_costmaps_){
      ROS_DEBUG_NAMED("move_base","Stopping costmaps initially");
      planner_costmap_ros_->stop();
      controller_costmap_ros_->stop();
    }

    //load any user specified recovery behaviors, and if that fails load the defaults
    //载入recovery behavior，这部分是状态机的设计问题，可以采用默认的设定，也可以用户指定
    if(!loadRecoveryBehaviors(private_nh)){
      loadDefaultRecoveryBehaviors();
    }

    //initially, we'll need to make a plan
    state_ = PLANNING;

    //we'll start executing recovery behaviors at the beginning of our list
    recovery_index_ = 0;

    //we're all set up now so we can start the action server
    as_->start();
    // 开启参数动态配置服务
    dsrv_ = new dynamic_reconfigure::Server<move_base::MoveBaseConfig>(ros::NodeHandle("~"));
    dynamic_reconfigure::Server<move_base::MoveBaseConfig>::CallbackType cb = boost::bind(&MoveBase::reconfigureCB, this, _1, _2);
    dsrv_->setCallback(cb);
  }

  void MoveBase::reconfigureCB(move_base::MoveBaseConfig &config, uint32_t level){
    boost::recursive_mutex::scoped_lock l(configuration_mutex_);

    //The first time we're called, we just want to make sure we have the
    //original configuration
    if(!setup_)
    {
      last_config_ = config;
      default_config_ = config;
      setup_ = true;
      return;
    }

    if(config.restore_defaults) {
      config = default_config_;
      //if someone sets restore defaults on the parameter server, prevent looping
      config.restore_defaults = false;
    }

    if(planner_frequency_ != config.planner_frequency)
    {
      planner_frequency_ = config.planner_frequency;
      p_freq_change_ = true;
    }

    if(controller_frequency_ != config.controller_frequency)
    {
      controller_frequency_ = config.controller_frequency;
      c_freq_change_ = true;
    }

    planner_patience_ = config.planner_patience;
    controller_patience_ = config.controller_patience;
    max_planning_retries_ = config.max_planning_retries;
    conservative_reset_dist_ = config.conservative_reset_dist;

    recovery_behavior_enabled_ = config.recovery_behavior_enabled;
    clearing_rotation_allowed_ = config.clearing_rotation_allowed;
    shutdown_costmaps_ = config.shutdown_costmaps;

    oscillation_timeout_ = config.oscillation_timeout;
    oscillation_distance_ = config.oscillation_distance;
    if(config.base_global_planner != last_config_.base_global_planner) {
      boost::shared_ptr<nav_core::BaseGlobalPlanner> old_planner = planner_;
      //initialize the global planner
      ROS_INFO("Loading global planner %s", config.base_global_planner.c_str());
      try {
        planner_ = bgp_loader_.createInstance(config.base_global_planner);

        // wait for the current planner to finish planning
        boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_);

        // Clean up before initializing the new planner
        planner_plan_->clear();
        latest_plan_->clear();
        controller_plan_->clear();
        resetState();
        planner_->initialize(bgp_loader_.getName(config.base_global_planner), planner_costmap_ros_);

        lock.unlock();
      } catch (const pluginlib::PluginlibException& ex) {
        ROS_FATAL("Failed to create the %s planner, are you sure it is properly registered and that the \
                   containing library is built? Exception: %s", config.base_global_planner.c_str(), ex.what());
        planner_ = old_planner;
        config.base_global_planner = last_config_.base_global_planner;
      }
    }

    if(config.base_local_planner != last_config_.base_local_planner){
      boost::shared_ptr<nav_core::BaseLocalPlanner> old_planner = tc_;
      //create a local planner
      try {
        tc_ = blp_loader_.createInstance(config.base_local_planner);
        // Clean up before initializing the new planner
        planner_plan_->clear();
        latest_plan_->clear();
        controller_plan_->clear();
        resetState();
        tc_->initialize(blp_loader_.getName(config.base_local_planner), &tf_, controller_costmap_ros_);
      } catch (const pluginlib::PluginlibException& ex) {
        ROS_FATAL("Failed to create the %s planner, are you sure it is properly registered and that the \
                   containing library is built? Exception: %s", config.base_local_planner.c_str(), ex.what());
        tc_ = old_planner;
        config.base_local_planner = last_config_.base_local_planner;
      }
    }

    make_plan_clear_costmap_ = config.make_plan_clear_costmap;
    make_plan_add_unreachable_goal_ = config.make_plan_add_unreachable_goal;

    last_config_ = config;
  }

  void MoveBase::goalCB(const geometry_msgs::PoseStamped::ConstPtr& goal){
    ROS_DEBUG_NAMED("move_base","In ROS goal callback, wrapping the PoseStamped in the action message and re-sending to the server.");
    move_base_msgs::MoveBaseActionGoal action_goal;
    action_goal.header.stamp = ros::Time::now();
    action_goal.goal.target_pose = *goal;

    action_goal_pub_.publish(action_goal);
  }

  void MoveBase::clearCostmapWindows(double size_x, double size_y){
    geometry_msgs::PoseStamped global_pose;

    //clear the planner's costmap
    getRobotPose(global_pose, planner_costmap_ros_); //获取在全局地图的pose，然后以这个点为中心，找到以size_x和size_y 为边长的矩形的四个顶点

    std::vector<geometry_msgs::Point> clear_poly;
    double x = global_pose.pose.position.x;
    double y = global_pose.pose.position.y;
    geometry_msgs::Point pt;

    pt.x = x - size_x / 2;
    pt.y = y - size_y / 2;
    clear_poly.push_back(pt);

    pt.x = x + size_x / 2;
    pt.y = y - size_y / 2;
    clear_poly.push_back(pt);

    pt.x = x + size_x / 2;
    pt.y = y + size_y / 2;
    clear_poly.push_back(pt);

    pt.x = x - size_x / 2;
    pt.y = y + size_y / 2;
    clear_poly.push_back(pt);

    planner_costmap_ros_->getCostmap()->setConvexPolygonCost(clear_poly, costmap_2d::FREE_SPACE);//完成对机器人所在区域的clear工作。

    //clear the controller's costmap
    getRobotPose(global_pose, controller_costmap_ros_); //同样的操作在controller_costmap_ros_ 上也操作一遍，这样关于global costmap 和local costmap都已经在机器人所在的区域完成clear工作。

    clear_poly.clear();
    x = global_pose.pose.position.x;
    y = global_pose.pose.position.y;

    pt.x = x - size_x / 2;
    pt.y = y - size_y / 2;
    clear_poly.push_back(pt);

    pt.x = x + size_x / 2;
    pt.y = y - size_y / 2;
    clear_poly.push_back(pt);

    pt.x = x + size_x / 2;
    pt.y = y + size_y / 2;
    clear_poly.push_back(pt);

    pt.x = x - size_x / 2;
    pt.y = y + size_y / 2;
    clear_poly.push_back(pt);

    controller_costmap_ros_->getCostmap()->setConvexPolygonCost(clear_poly, costmap_2d::FREE_SPACE);
  }

  // 会分别清除和重置局部和全局代价地图的所有层，最好不要高频率call clear_costmaps服务。一般是周期性调用，防止代价地图上有太多的garbage
  bool MoveBase::clearCostmapsService(std_srvs::Empty::Request &req, std_srvs::Empty::Response &resp){
    //clear the costmaps
    boost::unique_lock<costmap_2d::Costmap2D::mutex_t> lock_controller(*(controller_costmap_ros_->getCostmap()->getMutex()));
    controller_costmap_ros_->resetLayers();

    boost::unique_lock<costmap_2d::Costmap2D::mutex_t> lock_planner(*(planner_costmap_ros_->getCostmap()->getMutex()));
    planner_costmap_ros_->resetLayers();
    return true;
  }

  // req参数包含了起点和目标信息。首先判断global planner 的costmap是否存在以及是否能够得到机器人所在位置，
  bool MoveBase::planService(nav_msgs::GetPlan::Request &req, nav_msgs::GetPlan::Response &resp){
    if(as_->isActive()){
      ROS_ERROR("move_base must be in an inactive state to make a plan for an external user");
      return false;
    }
    //make sure we have a costmap for our planner
    if(planner_costmap_ros_ == NULL){
      ROS_ERROR("move_base cannot make a plan for you because it doesn't have a costmap");
      return false;
    }

    geometry_msgs::PoseStamped start;
    //if the user does not specify a start pose, identified by an empty frame id, then use the robot's pose
    if(req.start.header.frame_id.empty())
    {
        geometry_msgs::PoseStamped global_pose;
        if(!getRobotPose(global_pose, planner_costmap_ros_)){
          ROS_ERROR("move_base cannot make a plan for you because it could not get the start pose of the robot");
          return false;
        }
        start = global_pose;
    }
    else
    {
        start = req.start;
    }

    if (make_plan_clear_costmap_) {
      //update the copy of the costmap the planner uses
      clearCostmapWindows(2 * clearing_radius_, 2 * clearing_radius_);//完成对机器人区域的clear
    }

    //first try to make a plan to the exact desired goal
    std::vector<geometry_msgs::PoseStamped> global_plan;
    //逻辑判断这个调用是否成功，如果失败，则在目标区域附近搜索，更改req.goal的值，并重新调用makePlan，如果失败，则此次plan失败，无解。
    if(!planner_->makePlan(start, req.goal, global_plan) || global_plan.empty()){//完成plan计算的核心部分
      ROS_DEBUG_NAMED("move_base","Failed to find a plan to exact goal of (%.2f, %.2f), searching for a feasible goal within tolerance", req.goal.pose.position.x, req.goal.pose.position.y);

      //search outwards for a feasible goal within the specified tolerance
      geometry_msgs::PoseStamped p;
      p = req.goal;
      bool found_legal = false;
      float resolution = planner_costmap_ros_->getCostmap()->getResolution();
      float search_increment = resolution*3.0;
      if(req.tolerance > 0.0 && req.tolerance < search_increment) search_increment = req.tolerance;
      for(float max_offset = search_increment; max_offset <= req.tolerance && !found_legal; max_offset += search_increment) {
        for(float y_offset = 0; y_offset <= max_offset && !found_legal; y_offset += search_increment) {
          for(float x_offset = 0; x_offset <= max_offset && !found_legal; x_offset += search_increment) {

            //don't search again inside the current outer layer
            if(x_offset < max_offset-1e-9 && y_offset < max_offset-1e-9) continue;

            //search to both sides of the desired goal
            for(float y_mult = -1.0; y_mult <= 1.0 + 1e-9 && !found_legal; y_mult += 2.0) {

              //if one of the offsets is 0, -1*0 is still 0 (so get rid of one of the two)
              if(y_offset < 1e-9 && y_mult < -1.0 + 1e-9) continue;

              for(float x_mult = -1.0; x_mult <= 1.0 + 1e-9 && !found_legal; x_mult += 2.0) {
                if(x_offset < 1e-9 && x_mult < -1.0 + 1e-9) continue;

                p.pose.position.y = req.goal.pose.position.y + y_offset * y_mult;
                p.pose.position.x = req.goal.pose.position.x + x_offset * x_mult;

                if(planner_->makePlan(start, p, global_plan)){
                  if(!global_plan.empty()){

                    if (make_plan_add_unreachable_goal_) {
                      //adding the (unreachable) original goal to the end of the global plan, in case the local planner can get you there
                      //(the reachable goal should have been added by the global planner)
                      global_plan.push_back(req.goal);
                    }

                    found_legal = true;
                    ROS_DEBUG_NAMED("move_base", "Found a plan to point (%.2f, %.2f)", p.pose.position.x, p.pose.position.y);
                    break;
                  }
                }
                else{
                  ROS_DEBUG_NAMED("move_base","Failed to find a plan to point (%.2f, %.2f)", p.pose.position.x, p.pose.position.y);
                }
              }
            }
          }
        }
      }
    }

    //copy the plan into a message to send out
    resp.plan.poses.resize(global_plan.size());
    for(unsigned int i = 0; i < global_plan.size(); ++i){
      resp.plan.poses[i] = global_plan[i];
    }

    return true;
  }

  MoveBase::~MoveBase(){
    recovery_behaviors_.clear();

    delete dsrv_;

    if(as_ != NULL)
      delete as_;

    if(planner_costmap_ros_ != NULL)
      delete planner_costmap_ros_;

    if(controller_costmap_ros_ != NULL)
      delete controller_costmap_ros_;

    planner_thread_->interrupt();
    planner_thread_->join();

    delete planner_thread_;

    delete planner_plan_;
    delete latest_plan_;
    delete controller_plan_;

    planner_.reset();
    tc_.reset();
  }

  bool MoveBase::makePlan(const geometry_msgs::PoseStamped& goal, std::vector<geometry_msgs::PoseStamped>& plan){
    boost::unique_lock<costmap_2d::Costmap2D::mutex_t> lock(*(planner_costmap_ros_->getCostmap()->getMutex()));//首先lock 住global costmap
    plan.clear();                                                                   // 清空路径
    if(planner_costmap_ros_ == NULL) {                  // 如果没有全局代价地图，返回false
        ROS_ERROR("Planner costmap ROS is NULL, unable to create global plan");
        return false;
    }
    
    geometry_msgs::PoseStamped global_pose;// 获取机器人的起始位姿
    if(!getRobotPose(global_pose, planner_costmap_ros_)) {
      ROS_WARN("Unable to get starting pose of robot, unable to create global plan");
      return false;
    }

    const geometry_msgs::PoseStamped& start = global_pose;

    // 如果全局规划器失败了或者返回了一个零长度的路径，规划失败
    if(!planner_->makePlan(start, goal, plan) || plan.empty()){
      ROS_DEBUG_NAMED("move_base","Failed to find a  plan to point (%.2f, %.2f)", goal.pose.position.x, goal.pose.position.y);
      return false;
    }

    return true;
  }

  void MoveBase::publishZeroVelocity(){
    geometry_msgs::Twist cmd_vel;
    cmd_vel.linear.x = 0.0;
    cmd_vel.linear.y = 0.0;
    cmd_vel.angular.z = 0.0;
    vel_pub_.publish(cmd_vel);
  }

  bool MoveBase::isQuaternionValid(const geometry_msgs::Quaternion& q){
    // 先检查四元数的值有没有非法值(nan)或者无穷大的值(inf)
    if(!std::isfinite(q.x) || !std::isfinite(q.y) || !std::isfinite(q.z) || !std::isfinite(q.w)){
      ROS_ERROR("Quaternion has nans or infs... discarding as a navigation goal");
      return false;
    }

    tf2::Quaternion tf_q(q.x, q.y, q.z, q.w);

    // 接下来检查四元数的长度是不是趋于零
    if(tf_q.length2() < 1e-6){
      ROS_ERROR("Quaternion has length close to zero... discarding as navigation goal");
      return false;
    }

    // 归一化四元数
    tf_q.normalize();

    tf2::Vector3 up(0, 0, 1);

    double dot = up.dot(up.rotate(tf_q.getAxis(), tf_q.getAngle()));

    if(fabs(dot - 1) > 1e-3){
      ROS_ERROR("Quaternion is invalid... for navigation the z-axis of the quaternion must be close to vertical.");
      return false;
    }

    return true;
  }

  geometry_msgs::PoseStamped MoveBase::goalToGlobalFrame(const geometry_msgs::PoseStamped& goal_pose_msg){
    std::string global_frame = planner_costmap_ros_->getGlobalFrameID();
    geometry_msgs::PoseStamped goal_pose, global_pose;
    goal_pose = goal_pose_msg;

    //just get the latest available transform... for accuracy they should send
    //goals in the frame of the planner
    goal_pose.header.stamp = ros::Time();

    try{
      tf_.transform(goal_pose_msg, global_pose, global_frame);
    }
    catch(tf2::TransformException& ex){
      ROS_WARN("Failed to transform the goal pose from %s into the %s frame: %s",
          goal_pose.header.frame_id.c_str(), global_frame.c_str(), ex.what());
      return goal_pose_msg;
    }

    return global_pose;
  }

  void MoveBase::wakePlanner(const ros::TimerEvent& event){
    // we have slept long enough for rate
    planner_cond_.notify_one();// 通过boost thread的条件变量planner_cond_ 唤醒规划线程
  }
  //规划线程是MoveBase的核心函数，线程继续运行依赖于变量 runPlanner_
  //唤醒线程依赖于条件变量 planner_cond_ 这两个变量是后续其他函数需要唤醒并执行线程planThread的关键
  void MoveBase::planThread(){
    ROS_DEBUG_NAMED("move_base_plan_thread","Starting planner thread...");
    ros::NodeHandle n;
    ros::Timer timer;
    bool wait_for_wake = false;
    boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_);//首先将planner_mutex_ 锁住
    while(n.ok()){
      while(wait_for_wake || !runPlanner_){     // 确认是否要运行路径规划器(这里已经加锁)
        ROS_DEBUG_NAMED("move_base_plan_thread","Planner thread is suspending");        // 暂时关闭路径规划线程
        planner_cond_.wait(lock); // 等待条件满足。如果条件不满足，则释放锁，将线程置为waiting状态，继续等待；如果条件满足，则重新获取锁，结束wait，继续向下执行
        wait_for_wake = false;
      }
      ros::Time start_time = ros::Time::now();
      
      geometry_msgs::PoseStamped temp_goal = planner_goal_; //开始规划，复制路径规划器的目标点(注意这里在上次循环中加锁了)，然后在这次解锁。
      lock.unlock();
      ROS_DEBUG_NAMED("move_base_plan_thread","Planning...");
      
      planner_plan_->clear();
      bool gotPlan = n.ok() && makePlan(temp_goal, *planner_plan_);   //运行路径规划器，它的主要函数是makePlan

      //如果 makePlan 成功，则上锁，然后将 planner_plan_  拷贝给 latest_plan_ , 然后将上一次的 plan path 给 planner_plan_ ，这样这两个buffer就保存了最新的plan 和上一次的plan
      if(gotPlan){
        // 如果规划出路径则更新相应路径，并将state_设成CONTROLLING状态
        ROS_DEBUG_NAMED("move_base_plan_thread","Got Plan with %zu points!", planner_plan_->size());
        //pointer swap the plans under mutex (the controller will pull from latest_plan_)
        std::vector<geometry_msgs::PoseStamped>* temp_plan = planner_plan_;

        lock.lock();
        planner_plan_ = latest_plan_;
        latest_plan_ = temp_plan;                       // 将最新的全局路径放到latest_plan_中，其在MoveBase::executeCycle中被传递到controller_plan_中，用锁来进行同步
        last_valid_plan_ = ros::Time::now();
        planning_retries_ = 0;
        new_global_plan_ = true;

        ROS_DEBUG_NAMED("move_base_plan_thread","Generated a plan from the base_global_planner");

        // 如果没有到达目标点，进入CONTROLLING状态
        if(runPlanner_)
          state_ = CONTROLLING;
        if(planner_frequency_ <= 0)
          runPlanner_ = false;
        lock.unlock();
      }
      else if(state_==PLANNING){                    // 如果没有算出路径同时state_为PLANNING状态
        ROS_DEBUG_NAMED("move_base_plan_thread","No Plan...");
        ros::Time attempt_end = last_valid_plan_ + ros::Duration(planner_patience_);

        //check if we've tried to make a plan for over our time limit or our maximum number of retries
        //issue #496: we stop planning when one of the conditions is true, but if max_planning_retries_
        //is negative (the default), it is just ignored and we have the same behavior as ever
        // 判断是否超过最大规划周期或者规划次数，如果是则进入自转模式，否则应该会等待MoveBase::executeCycle的唤醒再次规划
        lock.lock();
        planning_retries_++;
        // 仅在MoveBase::executeCb及其调用的MoveBase::executeCycle或者重置状态时会被设置为PLANNING，一般是刚获得新目标，或者得到路径但计算不出下一步控制时重新进行路径规划
        if(runPlanner_ && (ros::Time::now() > attempt_end || planning_retries_ > uint32_t(max_planning_retries_))){
          // 进入障碍物清理模式
          state_ = CLEARING;
          runPlanner_ = false;  // proper solution for issue #523
          publishZeroVelocity();
          recovery_trigger_ = PLANNING_R;
        }

        lock.unlock();
      }

      // 加锁，下次循环中解锁
      lock.lock();

      // 如果还没到规划周期则定时器睡眠，在定时器中断中通过planner_cond_唤醒，这里规划周期为0
      if(planner_frequency_ > 0){
        ros::Duration sleep_time = (start_time + ros::Duration(1.0/planner_frequency_)) - ros::Time::now();
        if (sleep_time > ros::Duration(0.0)){
          wait_for_wake = true;
          timer = n.createTimer(sleep_time, &MoveBase::wakePlanner, this);
        }
      }
    }
  }

  // 控制的主要函数
  void MoveBase::executeCb(const move_base_msgs::MoveBaseGoalConstPtr& move_base_goal){
    if(!isQuaternionValid(move_base_goal->target_pose.pose.orientation)){         // 如果目标点朝向的四元数不合法，退出该函数，主要是pose.orientation检查
      as_->setAborted(move_base_msgs::MoveBaseResult(), "Aborting on goal because it was sent with an invalid quaternion");
      return;
    }
    
    geometry_msgs::PoseStamped goal = goalToGlobalFrame(move_base_goal->target_pose);   // 判断goal的有效性，统一转换到全局坐标系
    publishZeroVelocity();          // 发布零速度

    boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_);
    planner_goal_ = goal;
    runPlanner_ = true;// 全局规划标志位设为真
    // 由于全局规划器函数 plannerThread() 里有 planner_cond_ 对象的 wait 函数，在这里调用 notify 启动全局规划器线程，进行全局路径规划
    planner_cond_.notify_one();
    lock.unlock();

    current_goal_pub_.publish(goal);
    ros::Rate r(controller_frequency_); // 设置局部规划频率
    // 如果代价地图被关闭，则打开
    if(shutdown_costmaps_){
      ROS_DEBUG_NAMED("move_base","Starting up costmaps that were shut down previously");
      planner_costmap_ros_->start();
      controller_costmap_ros_->start();
    }

    //we want to make sure that we reset the last time we had a valid plan and control
    last_valid_control_ = ros::Time::now();               // 上一次的局部规划时间更新为当前时间
    last_valid_plan_ = ros::Time::now();                    // 上一次的全局规划时间更新为当前时间
    last_oscillation_reset_ = ros::Time::now();        // 上一次的震荡重置时间更新为当前时间
    planning_retries_ = 0;                                                  // 对同一目标的全局规划次数记录归为0

    ros::NodeHandle n;
    
    
    // 继续往下，判断goal的坐标系和planner_costmap_ros_ 的坐标系是否是一致的，如果不是，则调用goal = goalToGlobalFrame(goal) 
    // 转换到 planner_costmap_ros_ 的坐标系下，然后重新开启planThread。最终拿到了global plan，接下来调用核心代码：
    while(n.ok()){
      if(c_freq_change_){
        ROS_INFO("Setting controller frequency to %.2f", controller_frequency_);
        r = ros::Rate(controller_frequency_);                 // 更改控制频率
        c_freq_change_ = false;
      }

      // step xx 如果action_server被抢占，两个情况:
      // case 1. 局部规划过程中有了新的目标
      // case 2. 收到取消任务的命令

      // 抢占：进程间切换，低优先级进程放弃cpu，被高优先级进程占用
      // 阻塞：进程处于等待状态，不能继续运行，但可以被抢占。如果把抢占关闭，进行只能傻傻等待
      // 挂起：把被阻塞进程调离内存，“交换”到磁盘上的挂起队列上

      //  isPreemptRequested()这个成员函数用于检查是否有更高优先级的任务请求中断当前正在执行的动作目标。
      //  如果有客户端发送了预emption请求(即请求停止当前动作任务以便开始新的或更重要的任务)，则此函数会返回 true，否则返回 false
      if(as_->isPreemptRequested()){            // 首先判断 as_ 是否是可抢占的，如果请求抢占，则为 true，否则为 false
        // isNewGoalAvailable() 此方法检查是否有新的动作目标等待被处理。如果客户端已经发送了一个新的动作目标请求，
        // 并且服务器尚未接受和开始处理这个新目标，则此函数会返回 true。这对于服务器决定是否切换到新的动作目标或者继续执行当前目标非常重要
        // 上述的case 1
        if(as_->isNewGoalAvailable()){            // 检查是否有新的goal，如果有则处理新的goal
          // 有了新的可行目标后，新目标将会被执行，没有东西会被关闭
          // acceptNewGoal() 是 actionlib::SimpleActionServer 的成员函数，当有新的动作目标（goal）发送到服务器时，
          // 调用这个函数会接收并确认新的目标请求，并返回指向该新目标的引用或指针。
          move_base_msgs::MoveBaseGoal new_goal = *as_->acceptNewGoal();

          if(!isQuaternionValid(new_goal.target_pose.pose.orientation)){          // 判断 new goal 的合法性
            as_->setAborted(move_base_msgs::MoveBaseResult(), "Aborting on goal because it was sent with an invalid quaternion");           // setAborted()将活动目标的状态设置为中止
            return;
          }

          goal = goalToGlobalFrame(new_goal.target_pose);

          // 为下个执行循环重置状态
          recovery_index_ = 0;
          state_ = PLANNING;

          // 现在有新的目标点，路径规划的线程唤醒
          lock.lock();
          planner_goal_ = goal;
          runPlanner_ = true;
          planner_cond_.notify_one();       // 开启 规划线程 为new goal 生成plan
          lock.unlock();

          // 发布该目标点给rviz
          ROS_DEBUG_NAMED("move_base","move_base has received a goal of x: %.2f, y: %.2f", goal.pose.position.x, goal.pose.position.y);
          current_goal_pub_.publish(goal);

          // 相关的计时变量也要更新
          last_valid_control_ = ros::Time::now();
          last_valid_plan_ = ros::Time::now();
          last_oscillation_reset_ = ros::Time::now();
          planning_retries_ = 0;
        }
        else {
          // 上述的case 2： 如果任务被取消，重置导航各个部分的状态
          resetState();

          //notify the ActionServer that we've successfully preempted
          ROS_DEBUG_NAMED("move_base","Move base preempting the current goal");
          // 设置当前的 goal 为被强占状态
          // setPreempted() 当确认要响应预emption请求，即中断当前动作任务时，可以调用此方法。它会标记当前动作目标为“已预emption”，
          // 并通知所有关注此动作服务器状态的客户端，告知它们当前动作已被中断。同时，通常会在调用此方法前清理资源或执行必要的恢复操作
          as_->setPreempted();          // setPreempted() 将活动目标的状态设置为抢占

          //we'll actually return from execute after preempting
          return;
        }
      }

      // step xx 如果action_server未被抢占或被抢占的if结构已执行完毕，接下来开始局部规划，调用executeCycle函数，并记录局部控制起始时间
      // 先确认目标点的坐标系是不是和全局坐标系一致
      if(goal.header.frame_id != planner_costmap_ros_->getGlobalFrameID()){
        goal = goalToGlobalFrame(goal);

        // 进入planning状态，准备下个执行周期
        recovery_index_ = 0;
        state_ = PLANNING;

        // 有了新的目标点，所以需要确认路径规划器是唤醒的
        lock.lock();
        planner_goal_ = goal;
        runPlanner_ = true;
        planner_cond_.notify_one();
        lock.unlock();

        // 发布目标点给rviz
        ROS_DEBUG_NAMED("move_base","The global frame for move_base has changed, new frame: %s, new goal position x: %.2f, y: %.2f", goal.header.frame_id.c_str(), goal.pose.position.x, goal.pose.position.y);
        current_goal_pub_.publish(goal);

        // 相关的计时变量也要更新
        last_valid_control_ = ros::Time::now();
        last_valid_plan_ = ros::Time::now();
        last_oscillation_reset_ = ros::Time::now();
        planning_retries_ = 0;
      }
      
      ros::WallTime start = ros::WallTime::now();         // 开始为executeCycle函数计时，walltime计算的是客观的真实时间，而不是仿真里的时间
      bool done = executeCycle(goal);                               // 导航机器人到目标位置的主要工作是在executeCycle函数中完成的

      // 如果任务完成，则退出
      if(done)
        return;

      //check if execution of the goal has completed in some way

      ros::WallDuration t_diff = ros::WallTime::now() - start;
      ROS_DEBUG_NAMED("move_base","Full control cycle time: %.9f\n", t_diff.toSec());

      // 在每次循环开始时调用 r.sleep() 方法,它会自动计算并休眠足够的时间,确保从上一次调用到现在的时间间隔大约等于期望的循环周期时间(即1 / controller_frequency_ 秒),这样可以保持循环的稳定频率
      r.sleep();
      // r.cycleTime()： 这个方法返回自上次调用 sleep() 函数以来所经过的实际时间（单位：秒），即实际循环周期时间。
      // 如果实际循环周期时间大于期望的循环周期时间，说明在某个环节可能出现了阻塞或处理时间过长，导致无法按照设定的频率执行循环
      if(r.cycleTime() > ros::Duration(1 / controller_frequency_) && state_ == CONTROLLING)
        ROS_WARN("Control loop missed its desired rate of %.4fHz... the loop actually took %.4f seconds", controller_frequency_, r.cycleTime().toSec());
    }

    // 唤醒路径规划的线程方便退出
    lock.lock();
    runPlanner_ = true;
    planner_cond_.notify_one();
    lock.unlock();

    // 如果该节点被杀死，任务终止退出       setAborted()将活动目标的状态设置为中止
    as_->setAborted(move_base_msgs::MoveBaseResult(), "Aborting on the goal because the node has been killed");
    return;
  }

  double MoveBase::distance(const geometry_msgs::PoseStamped& p1, const geometry_msgs::PoseStamped& p2)
  {
    return hypot(p1.pose.position.x - p2.pose.position.x, p1.pose.position.y - p2.pose.position.y);
  }

  bool MoveBase::executeCycle(geometry_msgs::PoseStamped& goal){
    boost::recursive_mutex::scoped_lock ecl(configuration_mutex_);
    
    geometry_msgs::Twist cmd_vel;               // 用于发布速度命令
    geometry_msgs::PoseStamped global_pose;     // 获取机器人的当前位姿
    getRobotPose(global_pose, planner_costmap_ros_);
    const geometry_msgs::PoseStamped& current_position = global_pose;

    // 发布机器人当前位姿
    move_base_msgs::MoveBaseFeedback feedback;
    feedback.base_position = current_position;
    as_->publishFeedback(feedback);

    // 判断机器人是否走了足够的距离，如果是则重置震荡时间
    if(distance(current_position, oscillation_pose_) >= oscillation_distance_){
      last_oscillation_reset_ = ros::Time::now();
      oscillation_pose_ = current_position;

      // 如果上次的恢复行为是震荡导致的，重置恢复索引
      if(recovery_trigger_ == OSCILLATION_R)
        recovery_index_ = 0;
    }

    // 如果从costmap的buffer中观测数据不够新，则让机器人停止运动，退出函数
    if(!controller_costmap_ros_->isCurrent()){ //检查局部代价地图是否是最新的
      ROS_WARN("[%s]:Sensor data is out of date, we're not going to allow commanding of the base for safety",ros::this_node::getName().c_str());
      publishZeroVelocity();
      return false;
    }

    // 如果有了新的全局路径，把它传给局部规划器 检查是否有新的plan，如果有，则上锁递推 controller_plan_和 latest_plan_
    if(new_global_plan_){
      new_global_plan_ = false;
      ROS_DEBUG_NAMED("move_base","Got a new plan...swap pointers");
      // 全局路径的指针交换 controller_plan_和latest_plan_
      std::vector<geometry_msgs::PoseStamped>* temp_plan = controller_plan_;
      boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_);
      controller_plan_ = latest_plan_;
      latest_plan_ = temp_plan;
      lock.unlock();
      ROS_DEBUG_NAMED("move_base","pointers swapped!");

      if(!tc_->setPlan(*controller_plan_)){                   //将global的plan传递给tc_ 如果失败则关闭planThread并直接退出
        // ABORT and SHUTDOWN COSTMAPS
        ROS_ERROR("Failed to pass global plan to the controller, aborting.");
        resetState();
        
        lock.lock();
        runPlanner_ = false;                                                  // 关闭全局规划线程
        lock.unlock();
        as_->setAborted(move_base_msgs::MoveBaseResult(), "Failed to pass global plan to the controller.");
        return true;
      }

      // 更新recovery_index_
      if(recovery_trigger_ == PLANNING_R)
        recovery_index_ = 0;
    }

    // step xx move_base状态机，处理导航的控制逻辑，状态机的两个变量实现状态表示：{state_  recovery_trigger_}
    // 注意，这个状态机和planThread 是在交互的。状态机一旦进入PLANNING状态，就唤醒planThread线程，
    // 在planThread线程中一旦找到plan就将state_设置为CONTROLLING，如果没有找到plan，则state_ = CLEARING recovery_trigger_ = PLANNING_R
    switch(state_){
      // step x.x 如果是路径规划状态，计算路径
      case PLANNING:
      {
          // 加锁，唤醒路径规划器线程
          boost::recursive_mutex::scoped_lock lock(planner_mutex_);
          runPlanner_ = true;
          planner_cond_.notify_one();
        }
        ROS_DEBUG_NAMED("move_base","Waiting for plan, in the planning state.");
        break;

      // step x.x 如果是控制状态，尝试计算出有效的下发速度命令
      case CONTROLLING:
        ROS_DEBUG_NAMED("move_base","In controlling state.");

        //如果状态机在CONTROLLING状态，首先检查是否到达目的地
        if(tc_->isGoalReached()){
          ROS_DEBUG_NAMED("move_base","Goal reached!");
          resetState();

          // 关闭路径规划线程
          boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_);
          runPlanner_ = false;
          lock.unlock();

          as_->setSucceeded(move_base_msgs::MoveBaseResult(), "Goal reached.");           // setSucceeded()    Sets the status of the active goal to succeeded
          return true;
        }

        // step xx 检查是否满足震荡条件，
        // last_oscillation_reset_在一下几种情况都会被更新，
        // 1. 获得新目标         2. 距离超过震荡距离（默认0.5）         3. 进行recovery后         4. 执行executeCb时，全局路径和局部路径有效时
        // 较长时间没有发生以上情况会触发震荡，防止长时间卡在一个地方。oscillation_timeout_默认为0。
        if(oscillation_timeout_ > 0.0 && last_oscillation_reset_ + ros::Duration(oscillation_timeout_) < ros::Time::now())
        {
          publishZeroVelocity();
          state_ = CLEARING;
          recovery_trigger_ = OSCILLATION_R;
        }

        // step xx 如果一切正常，开始局部轨迹规划
        {
        // 锁住local costmap
        boost::unique_lock<costmap_2d::Costmap2D::mutex_t> lock(*(controller_costmap_ros_->getCostmap()->getMutex()));
        
        if(tc_->computeVelocityCommands(cmd_vel)){            // 就是从这个函数开始调用DWA/TEB算法 然后由baseLocalPlanner计算cmd_vel: 
          ROS_DEBUG_NAMED( "move_base", "Got a valid command from the local planner: %.3lf, %.3lf, %.3lf", cmd_vel.linear.x, cmd_vel.linear.y, cmd_vel.angular.z );
          last_valid_control_ = ros::Time::now();
          vel_pub_.publish(cmd_vel);                                                 // 下发速度控制给机器人底座
          if(recovery_trigger_ == CONTROLLING_R)
            recovery_index_ = 0;
        }
        else {
          ROS_DEBUG_NAMED("move_base", "The local planner could not find a valid plan.");
          ros::Time attempt_end = last_valid_control_ + ros::Duration(controller_patience_);

          // 判断控制命令的计算时间有没有超时
          if(ros::Time::now() > attempt_end){
            // 进入障碍物清除模式(一种恢复行为)
            publishZeroVelocity();
            state_ = CLEARING;
            recovery_trigger_ = CONTROLLING_R;
          }
          else{
            // 如果没有超时，启动路径规划器线程重新规划
            last_valid_plan_ = ros::Time::now();
            planning_retries_ = 0;
            state_ = PLANNING;
            publishZeroVelocity();

            // 使能全局规划的线程
            boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_);
            runPlanner_ = true;
            planner_cond_.notify_one();
            lock.unlock();
          }
        }
        }

        break;

      // step x.x 用提供的恢复行为来清理空间，主要有三个恢复行为
      case CLEARING:
        ROS_DEBUG_NAMED("move_base","In clearing/recovery state");
        // 不管什么恢复行为，只要使能了，就唤醒执行
        if(recovery_behavior_enabled_ && recovery_index_ < recovery_behaviors_.size()){
          ROS_DEBUG_NAMED("move_base_recovery","Executing behavior %u of %zu", recovery_index_+1, recovery_behaviors_.size());

          move_base_msgs::RecoveryStatus msg;
          msg.pose_stamped = current_position;
          msg.current_recovery_number = recovery_index_;
          msg.total_number_of_recoveries = recovery_behaviors_.size();
          msg.recovery_behavior_name =  recovery_behavior_names_[recovery_index_];

          recovery_status_pub_.publish(msg);

          recovery_behaviors_[recovery_index_]->runBehavior();/*重点内容*/

          // 更新震荡的计时时间
          last_oscillation_reset_ = ros::Time::now();

          // 检查恢复行为是否有效
          ROS_DEBUG_NAMED("move_base_recovery","Going back to planning state");
          last_valid_plan_ = ros::Time::now();
          planning_retries_ = 0;
          state_ = PLANNING;

          // 更新到下个恢复行为的索引
          recovery_index_++;
        }
        else{
          ROS_DEBUG_NAMED("move_base_recovery","All recovery behaviors have failed, locking the planner and disabling it.");
          // 关闭全局规划节点
          boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_);
          runPlanner_ = false;
          lock.unlock();

          ROS_DEBUG_NAMED("move_base_recovery","Something should abort after this.");
          
          if(recovery_trigger_ == CONTROLLING_R){
            ROS_ERROR("Aborting because a valid control could not be found. Even after executing all recovery behaviors");
            as_->setAborted(move_base_msgs::MoveBaseResult(), "Failed to find a valid control. Even after executing recovery behaviors.");
          }
          else if(recovery_trigger_ == PLANNING_R){
            ROS_ERROR("Aborting because a valid plan could not be found. Even after executing all recovery behaviors");
            as_->setAborted(move_base_msgs::MoveBaseResult(), "Failed to find a valid plan. Even after executing recovery behaviors.");
          }
          else if(recovery_trigger_ == OSCILLATION_R){
            ROS_ERROR("Aborting because the robot appears to be oscillating over and over. Even after executing all recovery behaviors");
            as_->setAborted(move_base_msgs::MoveBaseResult(), "Robot is oscillating. Even after executing recovery behaviors.");
          }
          resetState();
          return true;
        }
        break;
      default:
        ROS_ERROR("This case should never be reached, something is wrong, aborting");
        resetState();
        // 关闭全局规划线程
        boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_);
        runPlanner_ = false;
        lock.unlock();
        as_->setAborted(move_base_msgs::MoveBaseResult(), "Reached a case that should not be hit in move_base. This is a bug, please report it.");
        return true;
    }

    // 走到这里代表还没完成
    return false;
  }

  bool MoveBase::loadRecoveryBehaviors(ros::NodeHandle node){
    XmlRpc::XmlRpcValue behavior_list;
    if(node.getParam("recovery_behaviors", behavior_list)){
      if(behavior_list.getType() == XmlRpc::XmlRpcValue::TypeArray){
        for(int i = 0; i < behavior_list.size(); ++i){
          if(behavior_list[i].getType() == XmlRpc::XmlRpcValue::TypeStruct){
            if(behavior_list[i].hasMember("name") && behavior_list[i].hasMember("type")){
              //check for recovery behaviors with the same name
              for(int j = i + 1; j < behavior_list.size(); j++){
                if(behavior_list[j].getType() == XmlRpc::XmlRpcValue::TypeStruct){
                  if(behavior_list[j].hasMember("name") && behavior_list[j].hasMember("type")){
                    std::string name_i = behavior_list[i]["name"];
                    std::string name_j = behavior_list[j]["name"];
                    if(name_i == name_j){
                      ROS_ERROR("A recovery behavior with the name %s already exists, this is not allowed. Using the default recovery behaviors instead.",
                          name_i.c_str());
                      return false;
                    }
                  }
                }
              }
            }
            else{
              ROS_ERROR("Recovery behaviors must have a name and a type and this does not. Using the default recovery behaviors instead.");
              return false;
            }
          }
          else{
            ROS_ERROR("Recovery behaviors must be specified as maps, but they are XmlRpcType %d. We'll use the default recovery behaviors instead.",
                behavior_list[i].getType());
            return false;
          }
        }

        //if we've made it to this point, we know that the list is legal so we'll create all the recovery behaviors
        for(int i = 0; i < behavior_list.size(); ++i){
          try{
            //check if a non fully qualified name has potentially been passed in
            if(!recovery_loader_.isClassAvailable(behavior_list[i]["type"])){
              std::vector<std::string> classes = recovery_loader_.getDeclaredClasses();
              for(unsigned int i = 0; i < classes.size(); ++i){
                if(behavior_list[i]["type"] == recovery_loader_.getName(classes[i])){
                  //if we've found a match... we'll get the fully qualified name and break out of the loop
                  ROS_WARN("Recovery behavior specifications should now include the package name. You are using a deprecated API. Please switch from %s to %s in your yaml file.",
                      std::string(behavior_list[i]["type"]).c_str(), classes[i].c_str());
                  behavior_list[i]["type"] = classes[i];
                  break;
                }
              }
            }

            boost::shared_ptr<nav_core::RecoveryBehavior> behavior(recovery_loader_.createInstance(behavior_list[i]["type"]));

            //shouldn't be possible, but it won't hurt to check
            if(behavior.get() == NULL){
              ROS_ERROR("The ClassLoader returned a null pointer without throwing an exception. This should not happen");
              return false;
            }

            //initialize the recovery behavior with its name
            behavior->initialize(behavior_list[i]["name"], &tf_, planner_costmap_ros_, controller_costmap_ros_);
            recovery_behavior_names_.push_back(behavior_list[i]["name"]);
            recovery_behaviors_.push_back(behavior);
          }
          catch(pluginlib::PluginlibException& ex){
            ROS_ERROR("Failed to load a plugin. Using default recovery behaviors. Error: %s", ex.what());
            return false;
          }
        }
      }
      else{
        ROS_ERROR("The recovery behavior specification must be a list, but is of XmlRpcType %d. We'll use the default recovery behaviors instead.",
            behavior_list.getType());
        return false;
      }
    }
    else{
      //if no recovery_behaviors are specified, we'll just load the defaults
      return false;
    }

    //if we've made it here... we've constructed a recovery behavior list successfully
    return true;
  }

  // 加载默认的恢复行为
  void MoveBase::loadDefaultRecoveryBehaviors(){
    recovery_behaviors_.clear();
    try{
      //we need to set some parameters based on what's been passed in to us to maintain backwards compatibility
      ros::NodeHandle n("~");
      n.setParam("conservative_reset/reset_distance", conservative_reset_dist_);
      n.setParam("aggressive_reset/reset_distance", circumscribed_radius_ * 4);

      // step 1 加载清除代价地图的恢复行为
      boost::shared_ptr<nav_core::RecoveryBehavior> cons_clear(recovery_loader_.createInstance("clear_costmap_recovery/ClearCostmapRecovery"));
      cons_clear->initialize("conservative_reset", &tf_, planner_costmap_ros_, controller_costmap_ros_);
      recovery_behavior_names_.push_back("conservative_reset");
      recovery_behaviors_.push_back(cons_clear);

      // step 2 加载原地旋转的恢复行为
      boost::shared_ptr<nav_core::RecoveryBehavior> rotate(recovery_loader_.createInstance("rotate_recovery/RotateRecovery"));
      if(clearing_rotation_allowed_){
        rotate->initialize("rotate_recovery", &tf_, planner_costmap_ros_, controller_costmap_ros_);
        recovery_behavior_names_.push_back("rotate_recovery");
        recovery_behaviors_.push_back(rotate);
      }

      // step 3 加载比较主动积极的代价地图重置行为
      boost::shared_ptr<nav_core::RecoveryBehavior> ags_clear(recovery_loader_.createInstance("clear_costmap_recovery/ClearCostmapRecovery"));
      ags_clear->initialize("aggressive_reset", &tf_, planner_costmap_ros_, controller_costmap_ros_);
      recovery_behavior_names_.push_back("aggressive_reset");
      recovery_behaviors_.push_back(ags_clear);

      // step 4 再来一次原地旋转
      if(clearing_rotation_allowed_){
        recovery_behaviors_.push_back(rotate);
        recovery_behavior_names_.push_back("rotate_recovery");
      }
    }
    catch(pluginlib::PluginlibException& ex){
      ROS_FATAL("Failed to load a plugin. This should not happen on default recovery behaviors. Error: %s", ex.what());
    }

    return;
  }

  void MoveBase::resetState(){
    // 关闭全局规划线程
    boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_);
    runPlanner_ = false;
    lock.unlock();

    // 重置状态机并停止机器人运动
    state_ = PLANNING;
    recovery_index_ = 0;
    recovery_trigger_ = PLANNING_R;
    publishZeroVelocity();

    // 关闭代价地图
    if(shutdown_costmaps_){
      ROS_DEBUG_NAMED("move_base","Stopping costmaps");
      planner_costmap_ros_->stop();
      controller_costmap_ros_->stop();
    }
  }

  // 获取机器人的位姿，获取的过程不复杂，但是注意这里会检测各种可能异常，并抛出异常。是导航模块是否正常的检测过程
  bool MoveBase::getRobotPose(geometry_msgs::PoseStamped& global_pose, costmap_2d::Costmap2DROS* costmap)
  {
    tf2::toMsg(tf2::Transform::getIdentity(), global_pose.pose);
    geometry_msgs::PoseStamped robot_pose;
    tf2::toMsg(tf2::Transform::getIdentity(), robot_pose.pose);
    robot_pose.header.frame_id = robot_base_frame_;
    robot_pose.header.stamp = ros::Time(); // latest available
    ros::Time current_time = ros::Time::now();  // save time for checking tf delay later

    // 从代价地图中获得机器人位姿
    try
    {
      tf_.transform(robot_pose, global_pose, costmap->getGlobalFrameID());
    }
    catch (tf2::LookupException& ex)
    {
      ROS_ERROR_THROTTLE(1.0, "No Transform available Error looking up robot pose: %s\n", ex.what());
      return false;
    }
    catch (tf2::ConnectivityException& ex)
    {
      ROS_ERROR_THROTTLE(1.0, "Connectivity Error looking up robot pose: %s\n", ex.what());
      return false;
    }
    catch (tf2::ExtrapolationException& ex)
    {
      ROS_ERROR_THROTTLE(1.0, "Extrapolation Error looking up robot pose: %s\n", ex.what());
      return false;
    }

    // check if global_pose time stamp is within costmap transform tolerance
    if (current_time.toSec() - global_pose.header.stamp.toSec() > costmap->getTransformTolerance())
    {
      ROS_WARN_THROTTLE(1.0, "Transform timeout for %s. " \
                        "Current time: %.4f, pose stamp: %.4f, tolerance: %.4f", costmap->getName().c_str(),
                        current_time.toSec(), global_pose.header.stamp.toSec(), costmap->getTransformTolerance());
      return false;
    }

    return true;
  }
};
